/*
Robotics object oriented package in C++
Copyright (C) 2008-2009  Matrix

This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of the
License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received _a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston,  MA 02111-1307  USA
*/

#pragma once

#include "Chain.h"

namespace Robotic { namespace Mechanism
{
	using namespace Robotic::Math;

	/*
	Robot motion segment type.
	*/
	enum MotionSegmentType_Dyn
	{
		MotionSegment_Dyn_Joint,//this segment's desired target is in joint space.
		MotionSegment_Dyn_CartLinear,//this segment's desired target is in Cartesian space, and the trajectory is linear.
		MotionSegment_Dyn_CartCycle//this segment's desired target is in Cartesian space, and the trajectory is linear.
	};

	/*
	This motion trajectory is composed of several segments.
	*/
	class ROBOTIC_MECHANISM_API MotionSegment_Dyn
	{
	public:
		bool IsZone;//true means this motion segment is zone.
		MotionSegmentType_Dyn MotionSegmentType;//Motion segment type.
		ColumnVector TargetJoints;//Joint values of the target point, valid if MotionSegmentType is MotionSegment_Kine_Joint.
		Frame3 TargetFrame;//Cartesian frame of the target point w.r.t ObjectFrame, valid if MotionSegmentType is MotionSegment_Kine_CartLinear or MotionSegment_Kine_CartCycle.
		Frame3 ToolFrame;//Tool frame which will be moved to the TargetFrame, valid if MotionSegmentType is MotionSegment_Kine_CartLinear or MotionSegment_Kine_CartCycle.
		double Speed;//Desired motion speed of tool frame.
		double FlyByZone;//target's flyby zone.
	};
	ROBOTIC_MECHANISM_TEMPLATE template class ROBOTIC_MECHANISM_API std::vector<MotionSegment_Dyn>;

	/*
	*/
	class ROBOTIC_MECHANISM_API MotionPlanning_Dyn: public MotionPlanning
	{
	public:
		MotionPlanning_Dyn(Chain* pRobot, double stepInterval = 0.01);

		/*
		Destructor
		*/
		virtual ~MotionPlanning_Dyn();

		/*
		@brief: moves robot tool frame with specified velocity.
		@param vel: desired tool frame move velocity.
		@param global: if true, vel is w.r.t global frame, else it's w.r.t tool frame.
		*/
		virtual void MoveForward(Velocity vel, bool global = false);

		/*
		@brief: moves robot to make all joint values as toJointValues.
		@param toPoint: desired target joint values in joint space.
		@param speed: desired motion speed of joints. If it's <= 0, means maximum speed.
		@param flyByZone: if the motion command is followed by another motion command, 
		flyByZone is the error tolerance zone radius in Cartesian space when robot tool frame reaches to the toPoint. 
		It makes robot may pass any point inside the zone defined by toPoint and flyByZone with an appropriate speed.
		If there is no motion command followed, robot all joints will reach and stop at toPoint.
		*/
		virtual void MoveJoint(const ColumnVector toJointValues, double speed, double flyByZone = 0);

		/*
		@brief: moves robot tool frame to toPoint w.r.t object frame.
		@param toPoint: desired target frame w.r.t object frame.
		@param speed: desired motion speed of joints. If it's <= 0, means maximum speed.
		@param flyByZone: if the motion command is followed by another motion command, 
		flyByZone is the error tolerance zone radius in cartesian space when robot tool frame reaches to the toPoint. 
		It makes robot may pass any point inside the zone defined by toPoint and flyByZone with an appropriate speed.
		If there is no motion command followed, robot tool frame will reach and stop at toPoint.
		*/
		virtual void MoveCart(const Frame3 toPoint, double speed, double flyByZone = 0);

		/*
		@brief: moves robot tool frame to toPoint w.r.t object frame and make the path linearly.
		@param toPoint: desired target frame w.r.t object frame.
		@param speed: desired motion speed of ToolFrame. If it's <= 0, means maximum speed.
		@param flyByZone: if the motion command is followed by another motion command, 
		flyByZone is the error tolerance zone radius in cartesian space when robot tool frame reaches to the toPoint. 
		It makes robot may pass any point inside the zone defined by toPoint and flyByZone with an appropriate speed.
		If there is no motion command followed, robot tool frame will reach and stop at toPoint.
		*/
		virtual void MoveCart_Linear(const Frame3 toPoint, double speed, double flyByZone = 0);

		/*
		@brief: stops robot motion.
		@param followPath: true means robot will move along current path and reduce motion speed until 0;
		false means robot will stop as soon as possible without follow current path.
		*/
		virtual void Stop(const bool followPath);

		/*
		@brief: moves robot to next step and save joint values, velocities, accelerations and torques.
		@return: returns >0 means this motion step successful, 0 means motion completed and stopped, <0 means failed.
		@remark: after invokes this method, gets joint values, velocities, accelerations and torques with method GetCurRobotJointValues(), GetCurRobotJointVels(), GetCurRobotJointAccs(), GetCurRobotJointTorques().
		*/
		virtual int NextStep();
	private:
		std::vector<MotionSegment_Dyn> _motionSegments;
	};

}}